function [LogLikelihood,alphaDraws,alphatilde,alpha] = ...
    GeneralizedKFilterSimulationSmootherAJ(y,Z,d,H,T,c,R,Q,Harvey,numDraws,pvec)
%% GeneralizedKFilterSmoother.m
% GeneralizedKFilterSmoother.m is a version of the Kalman filter that
% augments the state when necessary to accomodate mixed frequency data
% and missing observations via the Harvey (1989) accumulator.
%  *Input:*
%    $y$ - matrix of the observables
%    $Z$ - structure for Z system matrix in the measurement equation
%    $H$ - structure for H system matrix of the measurement equation
%    $d$ - structure for d system vector of the measurement equation
%    $T$ - structure for T system matrix of the state equation
%    $c$ - structure for c system vector of the state equation
%    $R$ - structure for R system matrix of the state equation
%    $Q$ - structure for Q system matrix of the state equation
%    Harvey - structure to build accumulators into the state space model
%    Harvey.xi - vector of linear indices of mixed frequency series
%    Harvey.psi - matrix of full calendar of accumulator for each mixed
%                 frequency series
%    numDraws   - number of draws 
%    pvec       - percentile with the draws to report 
%                 defaults are [5 95] 
%  *Output:*
%    LogLikelihood    - loglikelihood value of the model
%    $\alpha$         - smoothed state of the model
%    $\tilde{a}_{0}$  - smoothed inference of initial condition
%
%
%    Copyright: Scott Brave and R. Andrew Butters (2013)
%
%% Diffuse Prior paramters
% $\kappa$ - diffuse prior parameter; see documentation
kappa = 10;
%% Calculate standard size parameters
% $p$ - number of observable series, $n$ - time series length
[p,n] = size(y);
%% Obtain system matrices from structure
% By passing both the different types of system matrices and the calendar
% vector corresponding to the particular type used at any particular time
% we allow for an arbitrary number of structural "breaks" in the state 
% space model. Furthermore, by allowing each system matrix/vector to have 
% its own calendar, no redundant matrices are saved in the workspace.

%% pvec 
if nargin < 11 || isempty(pvec)==true 
    pvec=[5 95]; 
else
    if any( pvec ) < 1
        warning('Requesting less than 1 percentile band')
        pause(1)
    end
    if any( pvec ) > 99.9999
        error('Requesting above 99.9999')
    end
end 
disp('_____________________________'); 
disp('Begin Simulation Smoother....'); 
sprintf('Percentiles requested =%3.1f \n',pvec)


% Z system matrix
if isstruct(Z)
    tauZ = Z.tauZ;
    Zt    = Z.Zt;
else
    tauZ = ones(1,n);
    Zt   = Z;
end
% d system matrix
if isstruct(d)
    taud = d.taud;
    dt    = d.dt;
else
    taud = ones(1,n);
    dt   = d;
end
% H system matrix
if isstruct(H)
    tauH = H.tauH;
    Ht    = H.Ht;
else
    tauH = ones(1,n);
    Ht   = H;
end
% T system matrix
if isstruct(T)
    tauT = T.tauT;
    Tt    = T.Tt;
else
    tauT = ones(1,n+1);
    Tt   = T;
end
% c system matrix
if isstruct(c)
    tauc = c.tauc;
    ct    = c.ct;
else
    tauc = ones(1,n+1);
    ct   = c;
end
% R system matrix
if isstruct(R)
    tauR = R.tauR;
    Rt    = R.Rt;
else
    tauR = ones(1,n+1);
    Rt   = R;
end
% Q system matrix
if isstruct(Q)
    tauQ = Q.tauQ;
    Qt    = Q.Qt;
else
    tauQ = ones(1,n+1);
    Qt   = Q;
end
%% Calculate transition equation size parameters
% $m$ - number of state variables, $g$ - number of shocks
[m,g] = size(Rt(:,:,end));
%% Augment Standard State Space with Harvey Accumulator
% $\xi$  - $\xi_{h}$ indice vector of each series accumulated
% $A$    - selection matrix
% $\psi$ - full history of accumulator indicator variable (for each series)
% $\tau$ - number of unique accumulator types
%        - Two Cases:'flow'    = 1 Standard Flow variable;
%                    'average' = 0 Time-averaged stock variable
if ~isempty(Harvey)
    xi = Harvey.xi;
    psi = Harvey.psi;
    
    type = any((psi==0),2);
    [s,r] = find((any((Zt(xi,:,:)~=0),3))');
    nonZeroZpos = zeros(length(xi),m);
    
    % Indentify unique elements of the state that need accumulation
    [APsi,~,nonZeroZpos(sub2ind([length(xi) m],r,s))] ...
        = unique([s type(r) psi(r,:)],'rows');
    
    tau = size(APsi,1);
    type = APsi(:,2);
    
    Ttypes   = [tauT' APsi(:,3:end)'];
    ctypes   = [tauc' APsi(:,3:end)'];
    Rtypes   = [tauR' APsi(:,3:end)'];
    
    [uniqueTs,~, newtauT] = unique(Ttypes,'rows');
    [uniquecs,~, newtauc] = unique(ctypes,'rows');
    [uniqueRs,~, newtauR] = unique(Rtypes,'rows');
    
    NumT = size(uniqueTs,1);
    Numc = size(uniquecs,1);
    NumR = size(uniqueRs,1);
    
    newT = zeros(m+tau,m+tau,NumT);
    for jj=1:NumT
        newT(1:m,:,jj) = [Tt(:,:,uniqueTs(jj,1)) zeros(m,tau)];
        for  h =1:tau
            if type(h) == 1
                newT(m+h,[1:m m+h],jj) = ...
                    [Tt(APsi(h,1),:,uniqueTs(jj,1)) ...
                    uniqueTs(jj,1+h)];
            else
                newT(m+h,[1:m m+h],jj) =...
                    [(1/uniqueTs(jj,1+h))*...
                    Tt(APsi(h,1),:,uniqueTs(jj,1))...
                    (uniqueTs(jj,1+h)-1)/uniqueTs(jj,1+h)];
            end
        end
    end
    
    newc = zeros(m+tau,Numc);
    for jj=1:Numc
        newc(1:m,jj) = ct(:,uniquecs(jj,1));
        for  h =1:tau
            if type(h) == 1
                newc(m+h,jj) = ct(APsi(h,1),uniquecs(jj,1));
            else
                newc(m+h,jj) = (1/uniquecs(jj,1+h))*...
                    ct(APsi(h,1),uniquecs(jj,1));
            end
        end
    end
    
    newR = zeros(m+tau,g,NumR);
    for jj=1:NumR
        newR(1:m,1:g,jj) = Rt(:,:,uniqueRs(jj,1));
        for  h =1:tau
            if type(h) == 1
                newR(m+h,:,jj) = Rt(APsi(h,1),:,uniqueRs(jj,1));
            else
                newR(m+h,:,jj) = (1/uniqueRs(jj,1+h))*...
                    Rt(APsi(h,1),:,uniqueRs(jj,1));
            end
        end
    end
    
    newZ = zeros(p,m+tau,size(Zt,3));
    for jj=1:size(Zt,3)
        newZ(:,1:m,jj) = Zt(:,:,jj);
        newZ(xi,:,jj) = zeros(length(xi),m+tau);
        for h=1:length(xi)
            cols = find(Zt(xi(h),:,jj));
            newZ(xi(h),m+nonZeroZpos(h,cols),jj) = Zt(xi(h),cols,jj);
        end
    end
    
    m    = m+tau;
    Tt   = newT;
    tauT = newtauT;
    Rt   = newR;
    tauR = newtauR;
    Zt   = newZ;
    ct   = newc;
    tauc = newtauc;
end
%% Clear unnecessary variables from workspace
clearvars -except y Zt tauZ dt taud Ht tauH Tt tauT ct tauc Rt tauR Qt...
    tauQ n p m g kappa NumberAccumulators NonZeroZPositions AccPositions...
    numDraws pvec
%% Establish Stationary & Non-stationary componenets of state
% Separate Cases into (i) Stationary
%
%
% $$a_{0} = \left(I_{m}-T_{\tau_{1}^{T}}\right)^{-1}c_{\tau_{1}^{c}}$$
%
%
% $$P_{0} = \left(I_{m^2}- T_{\tau_{1}^{T}} \otimes T_{\tau_{1}^{T}}
% \right)^{-1}\mbox{vec}(R_{\tau_{1}^{R}}Q_{\tau_{1}^{Q}}
% R_{\tau_{1}^{R}}^{\prime})$$
%
% (ii) Non-stationary state variables
%
% $$a_{0} = 0_{m\times 1}$$
%
%
% $$P_{0} = \kappa I_{m}$$
%
if all(abs(eig(Tt(:,:,tauT(1))))<0.99999)
    if all( ct==0 )==true
        a0=zeros(m,1);
    else
        a0 = inv(eye(m)-Tt(:,:,tauT(1)))*ct(:,tauc(1));
    end
    P0=lyapunov_symm( Tt(:,:,tauT(1)) ,...
        squeeze(Rt(:,:,tauR(1)))*squeeze(Qt(:,:,tauQ(1)))*squeeze(Rt(:,:,tauR(1)))' );
    P0=0.5*( P0 + P0' ); 
else
    a0 = zeros(m,1);
    P0 = kappa*eye(m); % Note preset kappa value in line 36 of this code.
end
%% Pre-allocate Storage Matrices
v    = zeros(p,n);
Finv = zeros(p,p,n);
K    = zeros(m,p,n);
L    = zeros(m,m,n);
P    = zeros(m,m,n+1);
a    = zeros(m,n+1);
LogL = zeros(1,n);
r    = zeros(m,n+1);
alpha = zeros(m,n);
eta   = zeros(g,n);
%% Initialize filter
%
% $$a_{1}  =  T_{\tau^{T}_{1}}a_{0}+c_{\tau^{c}_{1}}$$
%
%
% $$P_{1}  =  T_{\tau^{T}_{1}}P_{0}T_{\tau^{T}_{1}}^{\prime}+
% R_{\tau^{R}_{1}}Q_{\tau^{Q}_{1}}R_{\tau^{R}_{1}}^{\prime} $$
%
a(:,1) = Tt(:,:,tauT(1))*a0+ ct(:,tauc(1));
P(:,:,1) = Tt(:,:,tauT(1))*P0*Tt(:,:,tauT(1))'...
    +Rt(:,:,tauR(1))*Qt(:,:,tauQ(1))*Rt(:,:,tauR(1))';
%% Kalman Filter
%
% $$Z_{\tau^{Z}_{t}}^{\star}  =  W_{t}Z_{\tau^{Z}_{t}} $$
%
%
% $$H_{\tau^{H}_{t}}^{\star} = W_{t}H_{\tau^{H}_{t}}W_{t}^{\prime}$$
%
%
% $$ d_{\tau^{d}_{t}}^{\star} = W_{t}d_{\tau^{d}_{t}}$$
%
%
% $$v_{t}  =  y_{t} - Z^{\star}_{\tau^{Z}_{t}}a_{t} -
% d_{\tau^{d}_{t}}^{\star}$$
%
%
% $$F_{t}  = Z^{\star}_{\tau^{Z}_{t}}P_{t}Z^{\star\prime}_{\tau^{Z}_{t}}
% + H^{\star}_{\tau^{H}_{t}}$$
%
%
% $$K_{t} = T_{\tau^{T}_{t+1}}P_{t}Z^{\star\prime}_{\tau^{Z}_{t}}
% F_{t}^{-1}$$
%
%
% $$L_{t} = T_{\tau^{T}_{t+1}} - K_{t}Z^{\star}_{\tau^{Z}_{t}}$$
%
%
% $$a_{t+1} = T_{\tau^{T}_{t+1}}a_{t}+c_{\tau^{c}_{t+1}} + K_{t}v_{t}$$
%
%
% $$P_{t+1}  =  T_{\tau^{T}_{t+1}}P_{t}L_{t}^{\prime} +
% R_{\tau^{R}_{t+1}}Q_{\tau^{Q}_{t+1}}R_{\tau^{R}_{t+1}}^{\prime}$$
%
for ii = 1:n   
    % Create W matrix for possible missing values
    W = eye(p);
    ind = ~isnan(y(:,ii));
    W = W((ind==1),:);    
    v(ind,ii) = y(ind,ii) - (W*Zt(:,:,tauZ(ii)))*a(:,ii)-W*dt(:,taud(ii));       
    
    if isempty(Ht)==false
        tempF=(W*Zt(:,:,tauZ(ii)))*P(:,:,ii)*(W*Zt(:,:,tauZ(ii)))' ...
            + W*Ht(:,:,tauH(ii))*W';
    else
        tempF=(W*Zt(:,:,tauZ(ii)))*P(:,:,ii)*(W*Zt(:,:,tauZ(ii)))';
    end        
    tempF=0.5*(tempF+tempF');         
    [PSVD,DSDV,PSVDinv]=svd(tempF);
    
    %% 1.b Truncate small singular values
    tolZero=1e-12;
    firstZero=min(find(diag(DSDV) < tolZero));
    if isempty(firstZero)==false
        PSVD=PSVD(:,1:firstZero-1);
        PSVDinv=PSVDinv(:,1:firstZero-1);
        DSDV=DSDV(1:firstZero-1,1:firstZero-1);
    end
    Finv(ind,ind,ii)=PSVD*(DSDV\eye(length(DSDV)))*PSVDinv';
    logDetF=sum(log(diag(DSDV)));

    %     fprintf('Iteration %d \n',ii);
    %     comparemat( Finv(ind,ind,ii), inv(tempF) );
    
    LogL(:,ii) = -1/2*(logDetF +...
        v(ind,ii)'*(  Finv(ind,ind,ii) )*v(ind,ii));
    K(:,ind,ii) = Tt(:,:,tauT(ii+1))*P(:,:,ii)*...
        (W*Zt(:,:,tauZ(ii)))'*Finv(ind,ind,ii);
    L(:,:,ii) = Tt(:,:,tauT(ii+1)) - K(:,ind,ii)*(W*Zt(:,:,tauZ(ii)));
    a(:,ii+1) = Tt(:,:,tauT(ii+1))*a(:,ii)+...
        ct(:,tauc(ii+1))+K(:,ind,ii)*v(ind,ii);
    P(:,:,ii+1) = Tt(:,:,tauT(ii+1))*P(:,:,ii)*L(:,:,ii)'+ ...
        Rt(:,:,tauR(ii+1))*Qt(:,:,tauQ(ii+1))*Rt(:,:,tauR(ii+1))';
        
end
%% Calculate Loglikelihood
%
% $$\mathcal{L} = - \frac{np}{2}\log(2\pi) -
% \frac{1}{2}(\log(|F_{t}|)+v_{t}^{\prime}F_{t}^{-1}v_{t})$$
%
LogLikelihood = -(sum(sum(isfinite(y)))/2)*log(2*pi) + sum(LogL);

disp('Done with forward filter');
%% Kalman Smoother

for ii = n:-1:1
    % Create W matrix for possible missing values
    W = eye(p);
    ind = ~isnan(y(:,ii));
    W = W((ind==1),:);
    
    r(:,ii) = (W*Zt(:,:,tauZ(ii)))'*Finv(ind,ind,ii)*v(ind,ii) + ...
        L(:,:,ii)'*r(:,ii+1);
    alpha(:,ii) = a(:,ii) + P(:,:,ii)*r(:,ii);
    eta(:,ii)    = Qt(:,:,tauQ(ii))*Rt(:,:,tauR(ii))'*r(:,ii);
end
disp('Done with backward filter');
clear PSVD DSDV PSVDinv tempF; 
%% Simulation Smoothing for the State Vector
% Simulates desired number of draws from the state vector by using the
% corrected means method of Durbin & Koopman (2012)
if numDraws > 0
    
    %% m=dim.states
    %% n=Nobs
    
    alphaPlus      = zeros(m,n,numDraws);
    alphaPlusCheck = zeros(m,n,numDraws);
    yPlus          = NaN(p,n,numDraws);
    alpha0Plus     = reshape((mvnrnd(repmat(a0',numDraws,1),P0))',[m 1 numDraws]);
    Phi            = zeros(n*g,n*g);
    Phi(sub2ind(size(Phi),reshape(kron(reshape(1:n*g,g,n),ones(1,g)),[],1),kron((1:n*g)',ones(g,1)))) ...
        = reshape(Qt(:,:,tauQ(1:n)),[],1);
    etaPlus        = reshape((mvnrnd(zeros(numDraws,g*n),Phi))',[g n numDraws]);
    
    % Create W matrix for possible missing values
    W = eye(p);
    ind = ~isnan(y(:,1));
    W = W((ind==1),:);
    alphaPlus(:,1,:) = Tt(:,:,tauT(1))*squeeze(alpha0Plus(:,1,:)) ...
        + repmat(ct(:,tauc(1)),1,numDraws) + Rt(:,:,tauR(1))*squeeze(etaPlus(:,1,:));
    yPlus(ind,1,:) = (W*Zt(:,:,tauZ(1)))*squeeze(alphaPlus(:,1,:)) + repmat(W*dt(:,taud(1)),1,numDraws);
    for jj = 2:n
        % Create W matrix for possible missing values
        W = eye(p);
        ind = ~isnan(y(:,jj));
        W = W((ind==1),:);
        
        alphaPlus(:,jj,:) = Tt(:,:,tauT(jj))*squeeze(alphaPlus(:,jj-1,:)) ...
            + repmat(ct(:,tauc(jj)),1,numDraws) + Rt(:,:,tauR(jj))*squeeze(etaPlus(:,jj,:));
        yPlus(ind,jj,:) = (W*Zt(:,:,tauZ(jj)))*squeeze(alphaPlus(:,jj,:))+...
            + repmat(W*dt(:,taud(jj)),1,numDraws); 
        
    end
    
    for ii = 1:numDraws
        vPlus    = zeros(p,n,numDraws);
        KPlus    = zeros(m,p,n);
        FPlus    = zeros(m,p,n); 
        FPlusinv = zeros(m,p,n);
        LPlus    = zeros(m,m,n);
        PPlus    = zeros(m,m,n+1);
        aPlus    = zeros(m,n+1);
        rPlus    = zeros(m,n+1);
        aPlus(:,1) = Tt(:,:,tauT(1))*alpha0Plus(:,1,ii) ...
            + ct(:,tauc(1)) + Rt(:,:,tauR(1))*etaPlus(:,1,ii);
        PPlus(:,:,1) = Tt(:,:,tauT(1))*P0*Tt(:,:,tauT(1))'...
            +Rt(:,:,tauR(1))*Qt(:,:,tauQ(1))*Rt(:,:,tauR(1))';
        for jj = 1:n
            % Create W matrix for possible missing values
            W = eye(p);
            ind = ~isnan(yPlus(:,jj,ii));
            W = W((ind==1),:);
            
            vPlus(ind,jj,ii) = yPlus(ind,jj,ii) - (W*Zt(:,:,tauZ(jj)))*aPlus(:,jj)-W*dt(:,taud(jj));

            % Andrew
            if isempty(Ht)==false
                FPlus(ind,ind,jj) = (W*Zt(:,:,tauZ(jj)))*PPlus(:,:,jj)*(W*Zt(:,:,tauZ(jj)))' ...
                    + W*Ht(:,:,tauH(jj))*W';
            else
                FPlus(ind,ind,jj) = (W*Zt(:,:,tauZ(jj)))*PPlus(:,:,jj)*(W*Zt(:,:,tauZ(jj)))';
            end
            KPlus(:,ind,jj) = Tt(:,:,tauT(jj+1))*PPlus(:,:,jj)*...
                (W*Zt(:,:,tauZ(jj)))'*inv(FPlus(ind,ind,jj));
            
            %             % Me
            %             if isempty(Ht)==false
            %                 tempF=(W*Zt(:,:,tauZ(jj)))*PPlus(:,:,jj)*(W*Zt(:,:,tauZ(jj)))' ...
            %                     + W*Ht(:,:,tauH(jj))*W';
            %             else
            %                 tempF=(W*Zt(:,:,tauZ(jj)))*PPlus(:,:,jj)*(W*Zt(:,:,tauZ(jj)))';
            %             end
            %             tempF=0.5*(tempF+tempF');
            %             [PSVD,DSDV,PSVDinv]=svd(tempF);
            %
            %             %% 1.b Truncate small singular values
            %             tolZero=1e-12;
            %             firstZero=min(find(diag(DSDV) < tolZero));
            %             if isempty(firstZero)==false
            %                 PSVD=PSVD(:,1:firstZero-1);
            %                 PSVDinv=PSVDinv(:,1:firstZero-1);
            %                 DSDV=DSDV(1:firstZero-1,1:firstZero-1);
            %             end
            %             FPlusinv(ind,ind,ii)=PSVD*(DSDV\eye(length(DSDV)))*PSVDinv';
            %             KPlus(:,ind,jj) = Tt(:,:,tauT(jj+1))*PPlus(:,:,jj)*...
            %                 (W*Zt(:,:,tauZ(jj)))'*FPlusinv(ind,ind,jj);
            %
            %
            
            
            
            LPlus(:,:,jj) = Tt(:,:,tauT(jj+1)) - KPlus(:,ind,jj)*(W*Zt(:,:,tauZ(jj)));
            
            aPlus(:,jj+1) = Tt(:,:,tauT(jj+1))*aPlus(:,jj)+...
                ct(:,tauc(jj+1))+KPlus(:,ind,jj)*vPlus(ind,jj,ii);
            
            PPlus(:,:,jj+1) = Tt(:,:,tauT(jj+1))*PPlus(:,:,jj)*LPlus(:,:,jj)'+ ...
                Rt(:,:,tauR(jj+1))*Qt(:,:,tauQ(jj+1))*Rt(:,:,tauR(jj+1))';
            
        end
        for jj = n:-1:1
            % Create W matrix for possible missing values
            W = eye(p);
            ind = ~isnan(yPlus(:,jj,ii));
            W = W((ind==1),:);
            
            % Andrew
            rPlus(:,jj)     = (W*Zt(:,:,tauZ(jj)))'*inv(FPlus(ind,ind,jj))*vPlus(ind,jj)+...
               LPlus(:,:,jj)'*rPlus(:,jj+1);
            
            %% Me 
%             rPlus(:,jj)=(W*Zt(:,:,tauZ(jj)))'*FPlusinv(ind,ind,jj)*...
%                 vPlus(ind,jj,ii)+...
%                 LPlus(:,:,jj)'*rPlus(:,jj+1);
            
            alphaPlusCheck(:,jj,ii) = aPlus(:,jj) + PPlus(:,:,jj)*rPlus(:,jj);
        end
    end
%     alphatilde = reshape(alphaPlus,[],numDraws) - ...
%         reshape(alphaPlusCheck,[],numDraws) + repmat(reshape(alpha,[],1),1,numDraws);
%             
%     alphatilde=reshape( alphatilde,n,m,numDraws );

    alphatilde=alphaPlus - alphaPlusCheck + repmat(alpha,[1,1,numDraws]); 
    

    %% Compute Mean, Median and Percentiles     
    alphaDraws.mean=mean(alphatilde,3);
    alphaDraws.median=median(alphatilde,3);
    alphaDraws.bands=zeros( [size(alpha) length(pvec) ] );
    alphatilde =sort(alphatilde,3);
    pextract=round( numDraws*(pvec/100) );
    for jj=1:length(pvec); 
        alphaDraws.bands(:,:,jj)=alphatilde(:,:,pextract(jj) ); 
    end 
            
    alphaDraws.bands=permute( alphaDraws.bands,[2 1 3]); 
    
        
    
    %alphaDraws.std=std( alphatilde , 3);
    %     for jj=1:length(pvec);
    %         if rem( pvec(jj) , 1 )~=0
    %             fldnm=regexprep(sprintf('%2.1f',pvec(jj)),'.','_');
    %         else
    %             fldnm=sprintf('%2.0f',pvec(jj));
    %         end
    %         fldnm=['p',regexprep(fldnm,' ','')];
    %         alphaDraws.percentiles.(fldnm)=alphatilde(:,:,pextract(jj) );
    %     end
    
else
    alphatilde = [];
    alphaDraws=[]; 
end
alpha=alpha'; 
%% End of File
end